#include "data_reflow/data_buffer.h"

namespace data_reflow
{
    // ------------------------------ 静态单例指针初始化（关键！）------------------------------
    std::unique_ptr<DataBuffer> DataBuffer::instance_ = nullptr;

    // ------------------------------ 单例获取接口 ------------------------------
    DataBuffer& DataBuffer::get_instance()
    {
        static std::once_flag s_flag;
        std::call_once(s_flag, []() {
            instance_.reset(new DataBuffer());  // 仅初始化一次
        });
        return *instance_;
    }

    // ------------------------------ 构造函数 ------------------------------
    DataBuffer::DataBuffer()
    {
        buffer_size_ = loadBufferSizeFromConfig();
        RCLCPP_INFO(rclcpp::get_logger("DataBuffer"), "单例初始化完成，缓存容量：%lu 帧",
                     static_cast<unsigned long>(buffer_size_));
    }

    // ------------------------------ 工具函数 ------------------------------
    bool DataBuffer::isValidSensorType(SensorType type) const
    {
        return (type == SensorType::TYPE_CAMERA || type == SensorType::TYPE_LIDAR);
    }

    size_t DataBuffer::loadBufferSizeFromConfig() const
    {
        auto& config = ConfigManager::get_instance();
        if (!config.is_loaded())
        {
            RCLCPP_WARN(rclcpp::get_logger("DataBuffer"), "配置未加载，使用默认缓存容量 1000");
            return 1000;
        }
        int config_size = config.get_int("buffer_size", 1000);
        if (config_size <= 0 || config_size > 10000)
        {
            RCLCPP_WARN(rclcpp::get_logger("DataBuffer"), "配置容量无效（%d），使用默认值 1000", config_size);
            return 1000;
        }
        return static_cast<size_t>(config_size);
    }

    // ------------------------------ 核心功能实现 ------------------------------
    void DataBuffer::addData(const data_reflow_interfaces::msg::SensorData& buffer_data)
    {
        std::lock_guard<std::mutex> lock(buffer_mutex_);
        SensorType type = static_cast<SensorType>(buffer_data.type);

        if (!isValidSensorType(type))
        {
            RCLCPP_ERROR(rclcpp::get_logger("DataBuffer"), "无效传感器类型（%d），数据丢弃", buffer_data.type);
            return;
        }

        auto& deque = type_buffers_[type];
        if (deque.size() >= buffer_size_)
        {
            deque.pop_front();  // 容量满，删除最旧数据
        }
        deque.push_back(buffer_data);

        RCLCPP_DEBUG(rclcpp::get_logger("DataBuffer"), "已存储数据：类型%d，计数%u，时间戳%d.%09u，缓存量：%lu/%lu",
                     static_cast<int>(type), buffer_data.count,
                     buffer_data.stamp.sec, buffer_data.stamp.nanosec,
                     static_cast<unsigned long>(deque.size()), static_cast<unsigned long>(buffer_size_));
    }

    std::vector<data_reflow_interfaces::msg::SensorData> DataBuffer::queryByTypeAndTime(
        SensorType type, const rclcpp::Time& start_time, const rclcpp::Time& end_time)
    {
        std::lock_guard<std::mutex> lock(buffer_mutex_);
        std::vector<data_reflow_interfaces::msg::SensorData> result;

        if (!isValidSensorType(type))
        {
            RCLCPP_ERROR(rclcpp::get_logger("DataBuffer"), "查询失败：无效传感器类型（%d）", static_cast<int>(type));
            return result;
        }

        auto it = type_buffers_.find(type);
        if (it == type_buffers_.end() || it->second.empty())
        {
            RCLCPP_DEBUG(rclcpp::get_logger("DataBuffer"), "查询失败：类型%d无缓存数据", static_cast<int>(type));
            return result;
        }

        for (const auto& data : it->second)
        {
            rclcpp::Time data_time(data.stamp);
            if (data_time >= start_time && data_time <= end_time)
            {
                result.push_back(data);
            }
        }

        RCLCPP_INFO(rclcpp::get_logger("DataBuffer"), "查询完成：类型%d，时间范围[%f, %f]，匹配%lu条数据",
                     static_cast<int>(type), start_time.seconds(), end_time.seconds(),
                     static_cast<unsigned long>(result.size()));
        return result;
    }

    std::vector<data_reflow_interfaces::msg::SensorData> DataBuffer::getAllData(SensorType type)
    {
        std::lock_guard<std::mutex> lock(buffer_mutex_);
        std::vector<data_reflow_interfaces::msg::SensorData> result;

        if (!isValidSensorType(type))
        {
            RCLCPP_ERROR(rclcpp::get_logger("DataBuffer"), "获取全部数据失败：无效传感器类型（%d）", static_cast<int>(type));
            return result;
        }

        auto it = type_buffers_.find(type);
        if (it != type_buffers_.end() && !it->second.empty())
        {
            result.assign(it->second.begin(), it->second.end());
        }

        RCLCPP_INFO(rclcpp::get_logger("DataBuffer"), "获取全部数据完成：类型%d，共%lu条",
                     static_cast<int>(type), static_cast<unsigned long>(result.size()));
        return result;
    }

    void DataBuffer::clearByType(SensorType type)
    {
        std::lock_guard<std::mutex> lock(buffer_mutex_);
        if (!isValidSensorType(type))
        {
            RCLCPP_ERROR(rclcpp::get_logger("DataBuffer"), "清空失败：无效传感器类型（%d）", static_cast<int>(type));
            return;
        }

        auto it = type_buffers_.find(type);
        if (it != type_buffers_.end())
        {
            size_t count = it->second.size();
            it->second.clear();
            RCLCPP_INFO(rclcpp::get_logger("DataBuffer"), "清空完成：类型%d，删除%lu条数据",
                         static_cast<int>(type), static_cast<unsigned long>(count));
        }
        else
        {
            RCLCPP_WARN(rclcpp::get_logger("DataBuffer"), "清空跳过：类型%d无缓存数据", static_cast<int>(type));
        }
    }

    void DataBuffer::clearAll()
    {
        std::lock_guard<std::mutex> lock(buffer_mutex_);
        size_t total_count = 0;
        for (auto& [type, deque] : type_buffers_)
        {
            total_count += deque.size();
            deque.clear();
        }
        RCLCPP_INFO(rclcpp::get_logger("DataBuffer"), "全部清空完成：共删除%lu条数据", static_cast<unsigned long>(total_count));
    }
}  // namespace data_reflow